#pragma once
#include <iostream>
#include <Eigen/Dense>
#include <memory>
#include "continuous_linear_system.h"
#include "discrete_linear_system.h"
#include "kf.h"
#include "yaml-cpp/yaml.h"
#include <fstream>
using namespace KalmanFilter;
namespace auto_ros
{
namespace control
{
class KfVehicleBase
{
public:
	//init state fun when speed is 1
	KfVehicleBase(std::string kf_vehicle_yaml_path_name, double rate)
	{
		kf_yaml_ = YAML::LoadFile(kf_vehicle_yaml_path_name);
		std::fstream fout(kf_vehicle_yaml_path_name + ".cor", std::ios::out);
		fout << kf_yaml_;
		fout.close();
		rate_ = rate;
	}
	virtual bool step(Eigen::VectorXd &control_last, Eigen::VectorXd &y, double speed) = 0;
	virtual bool reset(Eigen::VectorXd state_esti, Eigen::MatrixXd state_cov_esti) = 0;
	virtual bool reset() = 0;
	virtual Eigen::VectorXd state_esti()
	{
		return kf_ptr_->state_esti();
	}
	virtual Eigen::MatrixXd state_cov_esti()
	{
		return kf_ptr_->state_cov_esti();
	}
	std::shared_ptr<ContinuousLinearSys> con_ptr_;
	std::shared_ptr<DiscreteLinearSys> dis_ptr_;
	std::shared_ptr<KF> kf_ptr_;

protected:
	YAML::Node kf_yaml_;
	double rate_;
};

} // namespace control
} // namespace auto_ros